clc;clear all;

Angle6of4_18=[-95.544,0.395,-100.189,9.473,96.414,-95.488;
              -98.408,-1.139,-99.683,13.226,99.556,-98.226;
              -102.924,0.519,-102.0,15.706,99.769,-97.659;
              -103.597,1.076,-103.795,14.637,101.391,-98.291;
              -94.358,4.001,-107.709,19.737,92.613,-89.729;
              -91.59,5-148,-109.655,19.602,94.224,-106.248;
              -91.3,-1.674,-100.401,9.459,90.678,-109.555;
              -84.862,-3.029,-98.892,8.991,87.475,-108.496;
              -80.734,-6.661,-96.087,9.879,90.788,-98.534;
              -82.821,-1.392,-101.841,10.382,88.114,-110.879;
              -76.469,-4.074,-98.759,9.022,84.625,-104.282;
              -70.349,-0.881,-106.05,11.085,76.488,-93.865;
              -67.508,-2.117,-100.046,4.287,74.24,-83.472;
              -62.858,2.014,-107.271,6.187,71.458,-78.16;
              -69.667,-4.304,-108.182,17.602,74.925,-85.737;
              -75.314,-8.377,-97.374,7.468,79.829,-85.994;
              -68.137,14.586,-125.447,30.351,73.343,-78.736;
              -78.576,16.47,-129.665,40.823,78.262,-99.622]; %机器人关节角
[rows,cols]=size(Angle6of4_18);
positivematrix=[];
rotatevect=[];

for i=1:rows
    GPTT06=forward_kinematics(Angle6of4_18(i,:)*pi/180);
    T06=PositiveSolution(Angle6of4_18(i,:)*pi/180);
    %旋转矩阵转为旋转向量
    R=T06(1:3,1:3);
    theta = acos((trace(R) - 1) / 2); % 计算旋转角度
    if sin(theta) ~= 0
        omega = 1/(2*sin(theta)) * [R(3,2) - R(2,3); R(1,3) - R(3,1); R(2,1) - R(1,2)]; % 计算旋转轴向量
    else
        omega = [0; 0; 0]; % 如果旋转角度为0，则旋转轴向量为零向量
    end
    omega = theta * omega; % 旋转向量即为旋转轴向量乘以旋转角度
    omega=(omega')*180/pi;%弧度转为角度
    rotatevect=[rotatevect;omega];
    T06=reshape(T06,4,4,1);
    positivematrix(:,:,i)=T06;

end

%%%相机外参矩阵
load('Camcalibration.mat');
calibration=[];
calibration_rotation=calibrationSession.CameraParameters.CameraParameters1.RotationMatrices;
calibration_trans=calibrationSession.CameraParameters.CameraParameters1.TranslationVectors;
calibration_trans=calibration_trans';

for i=1:11
    row=[0,0,0,1];
    calibration_now=[calibration_rotation(:,:,i) calibration_trans(:,i);row];
    calibration_now=reshape(calibration_now,4,4,1);
    calibration(:,:,i)=calibration_now;
    
end
gi2gj=[];
ci2cj=[];
for i=1:rows-1
    gi2gj_now=inv(positivematrix(:,:,i+1))*positivematrix(:,:,i);
    ci2cj_now=inv(calibration(:,:,i+1))*calibration(:,:,i);
    gi2gj(:,:,i)=gi2gj_now;
    ci2cj(:,:,i)=ci2cj_now;
end


%%%使用Tsai两步法
X= tsai(gi2gj,ci2cj);


function T = forward_kinematics(q)
    % UR5机器人的DH参数
    a = [0, -427, -357, 0, 0, 0];
    d = [147, 0, 0,147, 116, 105];
    alpha = [pi/2, 0,0, pi/2, -pi/2, 0];
    
    % 计算正运动学
    T = eye(4);
    for i = 1:6
        Ti = [cos(q(i)), -sin(q(i))*cos(alpha(i)), sin(q(i))*sin(alpha(i)), a(i)*cos(q(i));
              sin(q(i)), cos(q(i))*cos(alpha(i)), -cos(q(i))*sin(alpha(i)), a(i)*sin(q(i));
              0, sin(alpha(i)), cos(alpha(i)), d(i);
              0, 0, 0, 1];
        T = T * Ti;
    end
end


%%%%计算正解转换矩阵
function T06=PositiveSolution(angle)
    theta1=angle(1);
    theta2=angle(2);
    theta3=angle(3);
    theta4=angle(4);
    theta5=angle(5);
    theta6=angle(6);
    
    T01=[cos(theta1) -sin(theta1) 0 0 ;
        sin(theta1) cos(theta1) 0 0;
        0 0 1 147; 
        0 0 0 1 ];
    T12=[cos(theta2) -sin(theta2) 0 0;
        sin(theta2) cos(theta2) 0 0;
        0 0 1 0;
        0 0 0 1];
    T23=[cos(theta3) -sin(theta3) 0 -427 ;
        sin(theta3) cos(theta3) 0 0;
        0 0 1 0; 
         
        0 0 0 1];
    T34=[cos(theta4) -sin(theta4) 0 -357 ;
        sin(theta4) cos(theta4) 0 0;
        0 0 1 147; 
        
        0 0 0 1];
    T45=[cos(theta5) -sin(theta5) 0 0;
        
        0 0 -1 -116;
        sin(theta5) cos(theta5) 0 0;
        0 0 0 1];
    T56=[cos(theta6) -sin(theta6) 0 0;
        0 0 1 105;
        -sin(theta2) -cos(theta2) 0 0; 
        0 0 0 1];
    %{
    T01=[cos(theta1) -sin(theta1) 0 0 ; 
    sin(theta1) cos(theta1) 0 0; 
    0 0 1 147;
    0 0 0 1 ];
    T12=[cos(theta2) -sin(theta2) 0 0;
        0 0 -1 0;
        sin(theta2) cos(theta2) 0 0;
        0 0 0 1];
    T23=[cos(theta3) -sin(theta3) 0 -427 ; 
        sin(theta3) cos(theta3) 0 0; 
        0 0 1 0; 
        0 0 0 1];
    T34=[cos(theta4) -sin(theta4) 0 -357 ;
        sin(theta4) cos(theta4) 0 0;
        0 0 1 147; 
        0 0 0 1];
    T45=[cos(theta5) -sin(theta5) 0 0;
        0 0 -1 116;
        sin(theta5) cos(theta5) 0 0; 
        0 0 0 1];
    T56=[cos(theta6) -sin(theta6) 0 0;
        0 0 1 105;
        -sin(theta2) -cos(theta2) 0 0;
        0 0 0 1];
    %}
    T06=T01*T12*T23*T34*T45*T56;    
end



function X = tsai(A,B)
% Calculates the least squares solution of
% AX = XB
% 
% A New Technique for Fully Autonomous 
% and Efficient 3D Robotics Hand/Eye Calibration
% Lenz Tsai
% Mili Shah
% July 2014

[m,n] = size(A); n = n/4;
S = zeros(3*n,3);
v = zeros(3*n,1);

%Calculate best rotation R
for i = 1:n
    A1 = logm(A(1:3,4*i-3:4*i-1)); 
    B1 = logm(B(1:3,4*i-3:4*i-1));
    a = [A1(3,2) A1(1,3) A1(2,1)]';
    a = a/norm(a);
    b = [B1(3,2) B1(1,3) B1(2,1)]';
    b = b/norm(b);
    S(3*i-2:3*i,:) = skew(a+b);
    v(3*i-2:3*i,:) = a-b;
end
x = S\v;
theta = 2*atan(norm(x));
x = x/norm(x);
R = (eye(3)*cos(theta) + sin(theta)*skew(x) + (1-cos(theta))*(x*x'))';

%Calculate best translation t
C = zeros(3*n,3);
d = zeros(3*n,1);
I = eye(3);

for i = 1:n
    C(3*i-2:3*i,:) = I - A(1:3,4*i-3:4*i-1);
    d(3*i-2:3*i,:) = A(1:3,4*i)-R*B(1:3,4*i);
end

t = C\d;

%Put everything together to form X
X = [R t;0 0 0 1];

end

function Sk = skew( x )

   Sk = [0,-x(3),x(2);x(3),0,-x(1);-x(2),x(1),0];
   
end

